{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Drive with firetruck intrusion"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Hello there\n"
     ]
    }
   ],
   "source": [
    "print(\"Hello there\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "import carla\n",
    "import time\n",
    "import numpy as np\n",
    "import math\n",
    "import sys\n",
    "import cv2\n",
    "\n",
    "sys.path.append('D:\\CARLA_0.9.14\\WindowsNoEditor\\PythonAPI\\carla') # tweak to where you put carla\n",
    "from agents.navigation.global_route_planner import GlobalRoutePlanner\n",
    "\n",
    "# connect to the sim \n",
    "client = carla.Client('localhost', 2000)\n",
    "\n",
    "# define speed contstants\n",
    "PREFERRED_SPEED = 40 # what it says\n",
    "SPEED_THRESHOLD = 2 #defines when we get close to desired speed so we drop the\n",
    "\n",
    "# Max steering angle\n",
    "MAX_STEER_DEGREES = 40\n",
    "\n",
    "#camera mount offset on the car - you can tweak these to have the car in view or not\n",
    "CAMERA_POS_Z = 3 \n",
    "CAMERA_POS_X = -5 \n",
    "\n",
    "#adding params to display text to image\n",
    "font = cv2.FONT_HERSHEY_SIMPLEX\n",
    "# org - defining lines to display telemetry values on the screen\n",
    "org = (30, 30) # this line will be used to show current speed\n",
    "org2 = (30, 50) # this line will be used for future steering angle\n",
    "org3 = (30, 70) # and another line for future telemetry outputs\n",
    "org4 = (30, 90) # and another line for future telemetry outputs\n",
    "org3 = (30, 110) # and another line for future telemetry outputs\n",
    "fontScale = 0.5\n",
    "# white color\n",
    "color = (255, 255, 255)\n",
    "# Line thickness of 2 px\n",
    "thickness = 1\n",
    "\n",
    "\n",
    "# maintain speed function\n",
    "def maintain_speed(s):\n",
    "    ''' \n",
    "    this is a very simple function to maintan desired speed\n",
    "    s arg is actual current speed\n",
    "    '''\n",
    "    if s >= PREFERRED_SPEED:\n",
    "        return 0\n",
    "    elif s < PREFERRED_SPEED - SPEED_THRESHOLD:\n",
    "        return 0.9 # think of it as % of \"full gas\"\n",
    "    else:\n",
    "        return 0.4 # tweak this if the car is way over or under preferred speed \n",
    "\n",
    "#function to subtract 2 vectors\n",
    "def angle_between(v1, v2):\n",
    "    return math.degrees(np.arctan2(v1[1], v1[0]) - np.arctan2(v2[1], v2[0]))\n",
    "\n",
    "# function to get angle between the car and target waypoint\n",
    "def get_angle(car,wp):\n",
    "    '''\n",
    "    this function to find direction to selected waypoint\n",
    "    '''\n",
    "    vehicle_pos = car.get_transform()\n",
    "    car_x = vehicle_pos.location.x\n",
    "    car_y = vehicle_pos.location.y\n",
    "    wp_x = wp.transform.location.x\n",
    "    wp_y = wp.transform.location.y\n",
    "    \n",
    "    # vector to waypoint\n",
    "    x = (wp_x - car_x)/((wp_y - car_y)**2 + (wp_x - car_x)**2)**0.5\n",
    "    y = (wp_y - car_y)/((wp_y - car_y)**2 + (wp_x - car_x)**2)**0.5\n",
    "    \n",
    "    #car vector\n",
    "    car_vector = vehicle_pos.get_forward_vector()\n",
    "    degrees = angle_between((x,y),(car_vector.x,car_vector.y))\n",
    "\n",
    "    return degrees\n",
    "\n",
    "world = client.get_world()\n",
    "spawn_points = world.get_map().get_spawn_points()\n",
    "#look for a blueprint of Mini car\n",
    "vehicle_bp = world.get_blueprint_library().filter('*mini*')\n",
    "\n",
    "start_point = spawn_points[0]\n",
    "vehicle = world.try_spawn_actor(vehicle_bp[0], start_point)\n",
    "\n",
    "# create and show the navigation route like in Tutorial 3\n",
    "point_a = start_point.location #we start at where the car is\n",
    "sampling_resolution = 1\n",
    "grp = GlobalRoutePlanner(world.get_map(), sampling_resolution)\n",
    "# now let' pick the longest possible route\n",
    "distance = 0\n",
    "for loc in spawn_points: # we start trying all spawn points \n",
    "                            #but we just exclude first at zero index\n",
    "    cur_route = grp.trace_route(point_a, loc.location)\n",
    "    if len(cur_route)>distance:\n",
    "        distance = len(cur_route)\n",
    "        route = cur_route\n",
    "#draw the route in sim window - Note it does not get into the camera of the car\n",
    "for waypoint in route:\n",
    "    world.debug.draw_string(waypoint[0].transform.location, '^', draw_shadow=False,\n",
    "        color=carla.Color(r=0, g=0, b=255), life_time=60.0,\n",
    "        persistent_lines=True)\n",
    "\n",
    "# let's start a firetruck @ point 116 \n",
    "# so it could drive straight and knock our hero car off the navigation route\n",
    "# so we could see if the car will continue\n",
    "\n",
    "firetruck_bp = world.get_blueprint_library().filter('*fire*')\n",
    "firetruck_start_point = spawn_points[116]\n",
    "firetruck = world.try_spawn_actor(firetruck_bp[0], firetruck_start_point)\n",
    "    \n",
    "#setting RGB Camera - this follow the approach explained in a Carla video\n",
    "# link: https://www.youtube.com/watch?v=om8klsBj4rc&t=1184s\n",
    "\n",
    "camera_bp = world.get_blueprint_library().find('sensor.camera.rgb')\n",
    "camera_bp.set_attribute('image_size_x', '640') # this ratio works in CARLA 9.14 on Windows\n",
    "camera_bp.set_attribute('image_size_y', '360')\n",
    "\n",
    "camera_init_trans = carla.Transform(carla.Location(z=CAMERA_POS_Z,x=CAMERA_POS_X))\n",
    "#this creates the camera in the sim\n",
    "camera = world.spawn_actor(camera_bp,camera_init_trans,attach_to=vehicle)\n",
    "\n",
    "def camera_callback(image,data_dict):\n",
    "    data_dict['image'] = np.reshape(np.copy(image.raw_data),(image.height,image.width,4))\n",
    "\n",
    "image_w = camera_bp.get_attribute('image_size_x').as_int()\n",
    "image_h = camera_bp.get_attribute('image_size_y').as_int()\n",
    "\n",
    "camera_data = {'image': np.zeros((image_h,image_w,4))}\n",
    "# this actually opens a live stream from the camera\n",
    "camera.listen(lambda image: camera_callback(image,camera_data))\n",
    "\n",
    "cv2.namedWindow('RGB Camera',cv2.WINDOW_AUTOSIZE)\n",
    "cv2.imshow('RGB Camera',camera_data['image'])\n",
    "\n",
    "#main loop \n",
    "quit = False\n",
    "curr_wp = 5 #we will be tracking waypoints in the route and switch to next one wen we get close to current one\n",
    "predicted_angle = 0\n",
    "while curr_wp<len(route)-1:\n",
    "    # Carla Tick\n",
    "    world.tick()\n",
    "    if cv2.waitKey(1) == ord('q'):\n",
    "        quit = True\n",
    "        vehicle.apply_control(carla.VehicleControl(throttle=0,steer=0,brake=1))\n",
    "        break\n",
    "    image = camera_data['image']\n",
    "\n",
    "    # at a time when the Mini gets to a certain waypoint\n",
    "    # we get the truck going\n",
    "    # 150 value makes Mini crash into truck\n",
    "    # 165 makes the truck crash into the Mini\n",
    "    if curr_wp==166:\n",
    "        firetruck.apply_control(carla.VehicleControl(throttle=0.2,steer=0))\n",
    "    \n",
    "    while curr_wp < len(route) and vehicle.get_transform().location.distance(route[curr_wp][0].transform.location) < 5:\n",
    "        curr_wp +=1 #move to next wp if we are too close\n",
    "    \n",
    "    predicted_angle = get_angle(vehicle,route[curr_wp][0])\n",
    "    image = cv2.putText(image, 'Steering angle: '+str(round(predicted_angle,3)), org, font, fontScale, color, thickness, cv2.LINE_AA)\n",
    "    v = vehicle.get_velocity()\n",
    "    speed = round(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2),0)\n",
    "    image = cv2.putText(image, 'Speed: '+str(int(speed)), org2, font, fontScale, color, thickness, cv2.LINE_AA)\n",
    "    image = cv2.putText(image, 'Next wp: '+str(curr_wp), org3, font, fontScale, color, thickness, cv2.LINE_AA)\n",
    "    estimated_throttle = maintain_speed(speed)\n",
    "    # extra checks on predicted angle when values close to 360 degrees are returned\n",
    "    if predicted_angle<-300:\n",
    "        predicted_angle = predicted_angle+360\n",
    "    elif predicted_angle > 300:\n",
    "        predicted_angle = predicted_angle -360\n",
    "    steer_input = predicted_angle\n",
    "    # limit steering to max angel, say 40 degrees\n",
    "    if predicted_angle<-MAX_STEER_DEGREES:\n",
    "        steer_input = -MAX_STEER_DEGREES\n",
    "    elif predicted_angle>MAX_STEER_DEGREES:\n",
    "        steer_input = MAX_STEER_DEGREES\n",
    "    # conversion from degrees to -1 to +1 input for apply control function\n",
    "    steer_input = steer_input/75\n",
    "\n",
    "    vehicle.apply_control(carla.VehicleControl(throttle=estimated_throttle, steer=steer_input))\n",
    "    cv2.imshow('RGB Camera',image)\n",
    "    \n",
    "\n",
    "#clean up\n",
    "cv2.destroyAllWindows()\n",
    "camera.stop()\n",
    "for sensor in world.get_actors().filter('*sensor*'):\n",
    "    sensor.destroy()\n",
    "for actor in world.get_actors().filter('*vehicle*'):\n",
    "    actor.destroy()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
